package kit.edu.lego.gruppe3.levels;

import kit.edu.lego.gruppe3.ComponentsRepository;
import kit.edu.lego.gruppe3.actions.Movement;
import lejos.nxt.LCD;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Behavior;

public class MoveForward implements Behavior{
	
	private boolean suppressed = false;
	private DifferentialPilot robot = ComponentsRepository.getInstance().getRobotPilot();
	
	@Override
	public void action() {
		LCD.drawString("Now: Move forward", 0, 1);
		suppressed = false;


/*		Motor.A.setSpeed(100);
		Motor.C.setSpeed(100);
	    Motor.A.forward();
	    Motor.C.forward();
	    Movement.moveForward(360);
*/	    while( !suppressed ) 
	    	Thread.yield();
	    
//	    Motor.A.stop(); // clean up
//	    Motor.C.stop();
		Movement.stop();
		robot.setTravelSpeed(10);
		robot.forward();
	}

	@Override
	public void suppress() {
		suppressed = true;
	}

	@Override
	public boolean takeControl() {
		return true;
	}

}
